#ifndef VEYE_HPP
#define VEYE_HPP

#include "doublebf2.hpp"
#include "usbcamera.hpp"


#include <thread>
#include <iostream>

class VEye
{
public:
    USBCamera c;
    cv::Mat Knew;

    DoubleBf2<cv::Mat> frameBuffer;

    int k;
    bool play;

    std::thread captureThread;

    VEye()
        : k(0)
        , play(true)
    {

    }

    void End()
    {
        k=27;
        if(captureThread.joinable()) captureThread.join();
    }

    void Capture()
    {

        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
//        rv.at<double>(0)=20*CV_PI/180;

        rv.at<double>(0)=0*CV_PI/180;

        cv::Rodrigues(rv, R);


//        cv::Size sznew(3840,2160) ;
        cv::Size sznew(1920,1080);
        Knew=c.K.clone();
        Knew.at<double>(0,2)=sznew.width/2;
        Knew.at<double>(1,2)=sznew.height/2;


        cv::Mat map1, map2;
        cv::fisheye::initUndistortRectifyMap(c.K, c.D, R, Knew, sznew, CV_16SC2, map1, map2);


        //                std::cout<<"map1="<<map1.size()<<"\n"<<std::flush;
        //                std::cout<<"map2="<<map2.size()<<"\n"<<std::flush;


//        cv::Rect roi(960,0,1920,1080);
        cv::Rect roi(0,0,1920,1080);

        Knew.at<double>(0,2)-=roi.x;
        Knew.at<double>(1,2)-=roi.y;

        map1=map1(roi).clone();
        map2=map2(roi).clone();

        cv::Mat *pm=NULL;
        cv::Mat frame=cv::imread("/home/u/my_photo-2.jpg");
        while(k!=27)
        {
            if(play)
            {
                if(c.read(frame))
                {
                    pm=new cv::Mat();
                    cv::remap(frame, *pm, map1, map2, cv::INTER_LINEAR);
                    frameBuffer.Write(pm);
                }
                else
                {
                    break;
                }

            }
            std::this_thread::sleep_for(std::chrono::milliseconds(90));
            //        std::this_thread::yield();
        }
        c.release();

        pm=NULL;
        frameBuffer.Write(pm);
        std::cout<<"end capture\n"<<std::flush;
    }

    static void CaptureThread(VEye *pData)
    {
        pData->Capture();
    }


    void Display(std::string winname)
    {
        cv::namedWindow(winname,cv::WINDOW_NORMAL);
        cv::moveWindow(winname,32,32);
        cv::resizeWindow(winname, 1280, 720);

        while(k!=27)
        {
            cv::Mat *par=NULL;
            frameBuffer.Read(par);
            if(par!=NULL)
            {
                cv::imshow(winname, *par);
                k=cv::waitKey(10);
                delete par;
            }
            else
            {
                break;
            }
        }
        cv::destroyWindow(winname);
        std::cout<<"end Display\n"<<std::flush;
    }

    bool Wake(int mode=0)
    {
        bool res=c.open(mode);

        USBCamera::Load(c.K, "Kf.yml");
        USBCamera::Load(c.D, "Df.yml");

        USBCamera::ScaleK(c.K, c.get(CV_CAP_PROP_FRAME_WIDTH)/1280, c.get(CV_CAP_PROP_FRAME_HEIGHT)/720);




        if(res)
        {
            captureThread=std::thread(CaptureThread, this);
        }

        return res;
    }


    void test()
    {
        if(Wake(0))
        {
            play=true;

            Display("IDLE");
        }
        End();
    }

};

#endif // VEYE_HPP

